/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "dma.h"
#include "i2c.h"
#include "mylib/driver/uart.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include <mylib/utility/debug.h>
#include <mylib/device/motor.h>
#include <mylib/device/jy61p.h>
#include <mylib/device/oled.h>
#include <mylib/utility/pid.h>
#include <mylib/driver/stm32f1/board.h>
//#include <mylib/device/tof200f.h>
#include <mylib/device/led.h>



/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */


void iic_write_byte(uint8_t dev_addr, uint8_t reg_addr, uint8_t data) {}

oled_t g_oled = {.dev_addr = OLED_DEVICE_DEFAULT_ADDRESS, .iic_write_byte_func = iic_write_byte};

// 左轮电机 pwmA->PA6,in1->PB15,in2->PB14
motor_t g_lmotor = DEVICE_MOTOR_INITIALIZER(&htim3, TIM_CHANNEL_1, 0, 20000, GPIOA, GPIO_PIN_6,
                                            GPIOB, GPIO_PIN_15, MOTOR_DIRECTION_COROTATION_IN1);

// 右轮电机 pwmB->PA7,in1->PB13,in2->PB12
motor_t g_rmotor = {.timer       = &htim3,
                    .channel     = TIM_CHANNEL_2,
                    .max_peiod   = 20000,
                    .current_val = 0,
                    .port_in1    = GPIOB,
                    .pin_in1     = GPIO_PIN_13,
                    .port_in2    = GPIOB,
                    .pin_in2     = GPIO_PIN_12,
                    .rotate      = MOTOR_DIRECTION_COROTATION_IN1};

pid_position_t g_angle_pid;

float myfabs(float a)
{
    if (a < 0) {
        a = -a;
    }
    return a;
}

float calculate_yaw_error(float target_yaw, float current_yaw)
{
    float error = 0;
    if (target_yaw >= 0) {
        if (current_yaw <= 0) {
            if (myfabs(current_yaw) < (180 - target_yaw)) {
                error = myfabs(current_yaw) + target_yaw;
            }
            else {
                error = -(180 - target_yaw) - (180 - myfabs(current_yaw));
            }
        }
        else {
            if (current_yaw > 0) {
                error = target_yaw - current_yaw;
            }
        }
    }
    else if (target_yaw < 0) {
        if (current_yaw > 0) {
            if (current_yaw > target_yaw + 180) {
                error = (180 - current_yaw) + (180 - myfabs(current_yaw));
            }
            else if (current_yaw < target_yaw + 180) {
                error = -(myfabs(target_yaw) + current_yaw);
            }
        }
        else if (current_yaw < 0) {
            error = -(myfabs(target_yaw) - myfabs(current_yaw));
        }
    }
    return error;
}



bool end_flag= false;

void wait_one_frame(void)
{
    while(!end_flag);
}

jy61p_t jy61p;

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
    /* USER CODE BEGIN 1 */

    // uint8_t  ret;
    // uint16_t id;

    /* USER CODE END 1 */

    /* MCU Configuration--------------------------------------------------------*/

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* USER CODE BEGIN Init */

    /* USER CODE END Init */

    /* Configure the system clock */
    SystemClock_Config();

    /* USER CODE BEGIN SysInit */

    /* USER CODE END SysInit */

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_USART1_UART_Init();
    MX_USART2_UART_Init();
    MX_TIM3_Init();
    MX_ADC1_Init();
    MX_I2C1_Init();
    MX_TIM1_Init();
    MX_USART3_UART_Init();
    /* USER CODE BEGIN 2 */
   
    jy61p_init(&jy61p, UART_3);

    float acc_x, acc_y, acc_z;
    jy61p_get_acc(&jy61p, &acc_x, &acc_y, &acc_z);
    
    led_t led1, led2, led3, led4;
    led_init(&led1, PC13);
    led_init(&led2, PA2);
    led_init(&led3, PA3);
    led_init(&led4, PA4);

    led_on(&led1);


    // while(1){
    //     // 接收数据
    //     // HAL_UART_Receive_IT(&huart1, buf, 1);
    //     // 等待一帧数据接收完成
    //     wait_one_frame();
    // }
    
    // 初始化调试串口
    uart_bind(UART_1, &huart1);
    debug_init(UART_1);
    DEBUG_INFO("debug init success");

    // 初始化tof200f
    //tofxxf_init(&huart2);
    DEBUG_INFO("tof200f init success");
    while (1) {
        // int32_t distance = tofxxf_get_distance();
        // int32_t distance = -1;
        // DEBUG_INFO("distance = %d", distance);
        HAL_Delay(500);
    }

    // HAL_UARTEx_RxEventCallback()

    // 初始化OLED
    // oled_init(&g_oled);
    // 清屏
    // oled_clear(&g_oled);
    // 绘制字符
    // oled_draw_char(&g_oled, 1, 2, 'A');

    // // 设置基准pwm�?�?的compare value
    // int32_t base_lpwm = 2000;
    // int32_t base_rpwm = 2000;
    // // �?�?始陀螺仪偏航角置�?
    // jy61p_clear_yaw(&g_jy61p);
    // // 设置目标偏航�?
    // float target_yaw = 0;
    // // PID初始�?
    // pid_position_init(&g_angle_pid, 0.4, 0, 0, target_yaw);
    // while (1) {
    //     // 通过�?螺仪获取当前的偏航角
    //     float current_yaw = jy61p_get_yaw();
    //     // 计算目标偏航角和当前偏航角的偏差�?
    //     float error = calculate_yaw_error(target_yaw, current_yaw);
    //     // PID计算
    //     int32_t deviation = (int32_t)pid_position_calculate(&g_angle_pid, error / 1.8);
    //     // 形成差�?�，使其转弯
    //     int32_t lpwm = base_lpwm - deviation;
    //     int32_t rpwm = base_rpwm + deviation;
    //     // 设置电机占空�?
    //     motor_set_compare_val(&g_lmotor, lpwm);
    //     motor_set_compare_val(&g_rmotor, rpwm);
    //     // ...其他代码
    // }







    //    // 初始化ATK-MS53L1M
    //    ret = tof200_init(&huart2, &id);
    //    //    if (ret == ATK_MS53L1M_ERROR) {
    //    //        DEBUG_INFO("ATK-MS53L1M init failed!");
    //    //        return 0;
    //    //    }
    //    DEBUG_INFO("ATK-MS53L1M id = %d", id);

    /* USER CODE END 2 */

    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    //    uint16_t dat;
    //    while (1) {
    //        ret = atk_ms53l1m_modbus_get_data(id, &dat);

    //        if (ret == 0) { DEBUG_INFO("[Modbus]Distance: %dmm", dat); }
    //        else {
    //            DEBUG_INFO("Modbus mode get data failed!");
    //        }

    //        HAL_Delay(500);

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    //}
    /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef       RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef       RCC_ClkInitStruct = {0};
    RCC_PeriphCLKInitTypeDef PeriphClkInit     = {0};

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState       = RCC_HSE_ON;
    RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    RCC_OscInitStruct.HSIState       = RCC_HSI_ON;
    RCC_OscInitStruct.PLL.PLLState   = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource  = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLMUL     = RCC_PLL_MUL9;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
        Error_Handler();
    }
    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType =
        RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource   = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider  = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) {
        Error_Handler();
    }
    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
    PeriphClkInit.AdcClockSelection    = RCC_ADCPCLK2_DIV6;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
        Error_Handler();
    }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1) {}
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t* file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
